#ifndef TRACK_H_
#define TRACK_H_
#include "common/algorithm_type/lidar_cell_objs_type.h"
#include "common/algorithm_type/lidar_cell_type.h"
#include "common/algorithm_type/loc_type.h"
#include "km_match.h"
#include "match.h"
#include "match/Hungarian.h"
#include "match_center.h"
#include "rclcpp/rclcpp.hpp"
#include "utils.hpp"
#include <iostream>
#include <vector>
namespace perception
{
    namespace algorithm
    {
        /*用于计算l-shape的角点zd及l-shape的形状zs
            zd = 角点坐标，即四条边交点的最近点
            zs = 长、宽、航向
        */
        class Track
        {
        private:
            float euclidean_distance = 5;
            double pre_time = 0;
            double dt = 0.1; // 上下帧的时间差

            int max_cluster_size = 100;
            int min_cluster_size = 5;
            unsigned long int cclusters = 1;
            rclcpp::Node *private_nh_;
            track_config track_config_;
            std::shared_ptr<MatchKM> km_;
            std::vector<bool> ids;

            void CoordinateTransformation(std::vector<LidarObject> &cells,
                                          Localization &loc);
            void MatchHM(std::vector<LidarObject> &input_objs,
                         std::vector<MatchCenter> &now_objs, float &euclidean_distance,
                         std::vector<bool> &g_matched,
                         std::vector<bool> &c_matched,
                         std::vector<std::pair<int, int>> &pairs);

        public:
            Track(track_config &config, rclcpp::Node *private_nh);
            ~Track();
            int Process(std::vector<LidarObject> &objs, Localization &loc);
            int Process2(std::vector<LidarObject> &objs, Localization &loc);
            std::vector<Match> now_objs;
            std::vector<MatchCenter> now_objs_center;
        };

    } // namespace algorithm
} // namespace perception

#endif